#ifndef MATRIX4TEM_H
#define MATRIX4TEM_H

template <typename Real>
struct Matrix4Tem;

#include "imMaths.h"
#include "Vector3.h"
#include "Angle.h"

template <typename Real>
struct Matrix4Tem {
public:
	union {
		struct { // Row, Column
			Real _11, _12, _13, _14;
			Real _21, _22, _23, _24;
			Real _31, _32, _33, _34;
			Real _41, _42, _43, _44;
		};
		Real Data[4][4]; // Column, Row
		Real DataS[16];
	};

	//
	// Constructor
	Matrix4Tem(void) {
		memset(&Data, 0, sizeof(Real) * 16);
		//Identity();
	}
	
	//
	// Members
	void Identity(void) {
		memset(&Data, 0, sizeof(Real) * 16);
		_11 = _22 = _33 = _44 = 1.0;
	}

	Real Get2x2Determinant(int x1, int x2) {
		return (Data[x1][2] * Data[x2][3]) - (Data[x2][2] * Data[x1][3]);
	}
	Real GetDeterminant(void) {
		Real temp[12];
		// Col 0
		temp[ 0] = Get2x2Determinant(2, 3) * Data[1][1] * Data[0][0];
		temp[ 1] = Get2x2Determinant(1, 3) * Data[2][1] * Data[0][0];
		temp[ 2] = Get2x2Determinant(1, 2) * Data[3][1] * Data[0][0];
		// Col 1
		temp[ 3] = Get2x2Determinant(2, 3) * Data[0][1] * Data[1][0];
		temp[ 4] = Get2x2Determinant(0, 3) * Data[2][1] * Data[1][0];
		temp[ 5] = Get2x2Determinant(0, 2) * Data[3][1] * Data[1][0];
		// Col 2
		temp[ 6] = Get2x2Determinant(1, 3) * Data[0][1] * Data[2][0];
		temp[ 7] = Get2x2Determinant(0, 3) * Data[1][1] * Data[2][0];
		temp[ 8] = Get2x2Determinant(0, 1) * Data[3][1] * Data[2][0];
		// Col 3
		temp[ 9] = Get2x2Determinant(1, 2) * Data[0][1] * Data[3][0];
		temp[10] = Get2x2Determinant(0, 2) * Data[1][1] * Data[3][0];
		temp[11] = Get2x2Determinant(0, 1) * Data[2][1] * Data[3][0];

		Real Det = 0.0;
		for(int i = 0; i < 12; i+=2) {
			Det += temp[i];
			Det -= temp[i+1];
		};

		return Det;
	}

	void TransposeOf(Matrix4Tem<Real> &a) {
		for(int i = 0; i < 4; i++) {
			for(int j = 0; j < 4; j++) {
				Data[i][j] = a.Data[j][i];
			}
		}
	}

	// Based on this
	// http://www.cg.info.hiroshima-cu.ac.jp/~miyazaki/knowledge/teche23.html
	void InverseOf(Matrix4Tem<Real> &a) {
		Real Det = 1/a.GetDeterminant();

		_11 = (a._22 * a._33 * a._44) + (a._23 * a._34 * a._42) + (a._24 * a._32 * a._43) - (a._22 * a._34 * a._43) - (a._23 * a._32 * a._44)  - (a._24 * a._33 * a._42);
		_12 = (a._12 * a._34 * a._43) + (a._13 * a._32 * a._44) + (a._14 * a._33 * a._42) - (a._12 * a._33 * a._44) - (a._13 * a._34 * a._42)  - (a._14 * a._32 * a._43);
		_13 = (a._12 * a._23 * a._44) + (a._13 * a._24 * a._42) + (a._14 * a._22 * a._43) - (a._12 * a._24 * a._43) - (a._13 * a._22 * a._44)  - (a._14 * a._23 * a._42);
		_14 = (a._12 * a._24 * a._33) + (a._13 * a._22 * a._34) + (a._14 * a._23 * a._32) - (a._12 * a._23 * a._34) - (a._13 * a._24 * a._32)  - (a._14 * a._22 * a._33);

		_21 = (a._21 * a._34 * a._43) + (a._23 * a._31 * a._44) + (a._24 * a._33 * a._41) - (a._21 * a._33 * a._44) - (a._23 * a._34 * a._41)  - (a._24 * a._31 * a._43);
		_22 = (a._11 * a._33 * a._44) + (a._13 * a._34 * a._41) + (a._14 * a._31 * a._43) - (a._11 * a._34 * a._43) - (a._13 * a._31 * a._44)  - (a._14 * a._33 * a._41);
		_23 = (a._11 * a._24 * a._43) + (a._13 * a._21 * a._44) + (a._14 * a._23 * a._41) - (a._11 * a._23 * a._44) - (a._13 * a._24 * a._41)  - (a._14 * a._21 * a._43);
		_24 = (a._11 * a._23 * a._34) + (a._13 * a._24 * a._31) + (a._14 * a._21 * a._33) - (a._11 * a._24 * a._33) - (a._13 * a._21 * a._34)  - (a._14 * a._23 * a._31);
		
		_31 = (a._21 * a._32 * a._44) + (a._22 * a._34 * a._41) + (a._24 * a._31 * a._42) - (a._21 * a._34 * a._42) - (a._22 * a._31 * a._44)  - (a._24 * a._32 * a._41);
		_32 = (a._11 * a._34 * a._42) + (a._12 * a._31 * a._44) + (a._14 * a._32 * a._41) - (a._11 * a._32 * a._44) - (a._12 * a._34 * a._41)  - (a._14 * a._31 * a._42);
		_33 = (a._11 * a._22 * a._44) + (a._12 * a._24 * a._41) + (a._14 * a._21 * a._42) - (a._11 * a._24 * a._42) - (a._12 * a._21 * a._44)  - (a._14 * a._22 * a._41);
		_34 = (a._11 * a._24 * a._32) + (a._12 * a._21 * a._34) + (a._14 * a._22 * a._31) - (a._11 * a._22 * a._34) - (a._12 * a._24 * a._31)  - (a._14 * a._21 * a._32);
		
		_41 = (a._21 * a._33 * a._42) + (a._22 * a._31 * a._43) + (a._23 * a._32 * a._41) - (a._21 * a._32 * a._43) - (a._22 * a._33 * a._41)  - (a._23 * a._31 * a._42);
		_42 = (a._11 * a._32 * a._43) + (a._12 * a._33 * a._41) + (a._13 * a._31 * a._42) - (a._11 * a._33 * a._42) - (a._12 * a._31 * a._43)  - (a._13 * a._32 * a._41);
		_43 = (a._11 * a._23 * a._42) + (a._12 * a._21 * a._43) + (a._13 * a._22 * a._41) - (a._11 * a._22 * a._43) - (a._12 * a._23 * a._41)  - (a._13 * a._21 * a._42);
		_44 = (a._11 * a._22 * a._33) + (a._12 * a._23 * a._31) + (a._13 * a._21 * a._32) - (a._11 * a._23 * a._32) - (a._12 * a._21 * a._33)  - (a._13 * a._22 * a._31);
	
		for each(Real a in DataS)
			a *= Det;
	}
	//
	// Static members
	static Matrix4Tem<Real> Projection3D(Real NearZ, Real FarZ, Real FOV, Real AspectRatio) {
		Matrix4Tem<Real> m;
		Real SinFOV2 = sin(FOV*0.5);
		Real CosFOV2 = cos(FOV*0.5);

		Real w = (CosFOV2 / SinFOV2) / AspectRatio;
		Real h = (CosFOV2 / SinFOV2);
		Real q = FarZ / (FarZ - NearZ);

		m._11 = w;
		m._22 = h;
		m._33 = q;
		m._34 = 1.0;
		m._43 = -q * NearZ;
		m._44 = 0.0;

		return m;
	}

	static Matrix4Tem<Real> View(Vector3Tem<Real> &Position, AngleTem<Real> &Orientation) {
		Matrix4Tem<Real> m;

		Vector3Tem<Real> Right = Orientation.Right();
		Vector3Tem<Real> Up = Orientation.Up();
		Vector3Tem<Real> Dir = Orientation.Forward();

		m._14 = m._24 = m._34 = 0.0f;
		m._44 = 1.0f;

		m._11 = Right.x;
		m._21 = Right.y;
		m._31 = Right.z;
		m._41 = -Right.Dot(Position);

		m._12 = Up.x;
		m._22 = Up.y;
		m._32 = Up.z;
		m._42 = -Up.Dot(Position);

		m._13 = Dir.x;
		m._23 = Dir.y;
		m._33 = Dir.z;
		m._43 = -Dir.Dot(Position);

		return m;
	}

	static Matrix4Tem<Real> World(Vector3Tem<Real> &Position, AngleTem<Real> &Orientation) {
		Matrix4Tem<Real> m;

		Vector3Tem<Real> Forward, Right, Up;

		Forward = Orientation.Forward();
		Right = Orientation.Right();
		Up = Orientation.Up();


		m._11 = Right.x;
		m._12 = Right.y;
		m._13 = Right.z;

		m._21 = Up.x;
		m._22 = Up.y;
		m._23 = Up.z;

		m._31 = Forward.x;
		m._32 = Forward.y;
		m._33 = Forward.z;

		m._41 = Position.x;
		m._42 = Position.y;
		m._43 = Position.z;

		m._44 = 1.0f;

		return m;
	}

	static Matrix4Tem<Real> World(Vector3Tem<Real> &Position, QuaternionTem<Real> &Orientation) {
		Matrix4Tem<Real> m;
		Orientation.GetMatrix(&m);

		m._41 = Position.x;
		m._42 = Position.y;
		m._43 = Position.z;

		m._44 = 1.0f;

		return m;
	}

	//
	// Operators
	Matrix4Tem<Real> operator* (Real a) {
		Matrix4Tem r;

		for(char i = 0; i < 16; i++)
			r.DataS[i] = DataS[i] * a;

		return r;
	}
	Matrix4Tem<Real> operator*= (Real a) {
		for(char i = 0; i < 16; i++)
			DataS[i] *= a;

		return *this;
	}

	Matrix4Tem<Real> operator+ (const Matrix4Tem<Real> &m) {
		Matrix4Tem c;

		for(char i = 0; i < 16; i++)
			c.DataS[i] = DataS[i] + m.DataS[i];

		return c;
	}
	Matrix4Tem<Real> operator- (const Matrix4Tem<Real> &m) {
		Matrix4Tem c;

		for(int i = 0; i < 16; i++)
			c.DataS[i] = DataS[i] - m.DataS[i];

		return c;
	}
	Matrix4Tem<Real> operator* (const Matrix4Tem<Real> &m) {
		Matrix4Tem c;

		memset(&c, 0, sizeof(c));

		for(char i = 0; i < 4; i++)
			for(char j = 0; j < 4; j++)
				for(char k = 0; k < 4; k++)
					c.Data[i][j] += Data[i][k] * m.Data[k][j];

		return c;
	}

#ifdef _D3D9_H_
	operator D3DXMATRIX(void) {
		D3DXMATRIX a;
		memcpy(&a, this, sizeof(D3DXMATRIX));
		return a;
	}

	Matrix4Tem(D3DXMATRIX a) {
		memcpy(this, &a, sizeof(D3DXMATRIX));
	}
#endif
};

typedef Matrix4Tem<float> Matrix;
typedef Matrix4Tem<float> Matrix4;
typedef Matrix4Tem<float> Matrix4f;
typedef Matrix4Tem<double> Matrix4d;

#endif